;*********************************************************************************************************
;                                               OS_CPU_A.ASM
;
;   Copyright 2003, Validated Software Corporation, Lafayette, CO
;   All Rights Reserved
;
;   Author:     Mark Scott-Nash
;   Date:       03-Sep-03
;
;   References:
;   $Workfile: OS_CPU_A.ASM $
;
;   Description:    These are the assembly language level OS support routines
;                   for the TMS320C6416 port on Code Composer Studio
;
;   Contents:
;		unsigned int ReturnCurrentDP(void)	RETURN CURRENT DATA POINTER
;       unsigned int getStackPtr(void)      RETURN CURRENT STACK POINTER
;		void OSIntCtxSw(void)			OS CONTEXT SWITCH, INTERRUPT LEVEL
;		void OSCtxSw(void)			OS CONTEXT SWITCH
;		void OSStartHighRdy(void)               OS START HIGHEST PRIORITY TASK
;
;   Revision History - latest change on top
;
;   $Log: /UC03/Source/6414-DSP/src/OS_CPU_A.ASM $
;
;
;*********************************************************************************************************

           .data
           .global _OS_CPU_A_ASM
_OS_CPU_A_ASM: .string "$Header: /UC03/Source/6414-DSP/src/OS_CPU_A.ASM 8     3/15/04 9:02a Jelliott $"
           .byte 0
           .text


FP  .set    A15
DP	.set	B14
SP	.set	B15


; add 4 because SP points to next FREE location, these are SP relative addresses
_A0  .set  0        +4
_A1  .set  4        +4
_A2  .set  8        +4
_A3  .set  12       +4
_A4  .set  16       +4
_A5  .set  20       +4
_A6  .set  24       +4
_A7  .set  28       +4
_A8  .set  32       +4
_A9  .set  36       +4
_A10 .set  40       +4
_A11 .set  44       +4
_A12 .set  48       +4
_A13 .set  52       +4
_A14 .set  56       +4
_A15 .set  60       +4
_A16 .set  64       +4
_A17 .set  68       +4
_A18 .set  72       +4
_A19 .set  76       +4
_A20 .set  80       +4
_A21 .set  84       +4
_A22 .set  88       +4
_A23 .set  92       +4
_A24 .set  96       +4
_A25 .set  100      +4
_A26 .set  104      +4
_A27 .set  108      +4
_A28 .set  112      +4
_A29 .set  116      +4
_A30 .set  120      +4
_A31 .set  124      +4
_B0  .set  128      +4
_B1  .set  132      +4
_B2  .set  136      +4
_B3  .set  140      +4
_B4  .set  144      +4
_B5  .set  148      +4
_B6  .set  152      +4
_B7  .set  156      +4
_B8  .set  160      +4
_B9  .set  164      +4
_B10 .set  168      +4
_B11 .set  172      +4
_B12 .set  176      +4
_B13 .set  180      +4
_B14 .set  184      +4
_B15 .set  188      +4
_B16 .set  192      +4
_B17 .set  196      +4
_B18 .set  200      +4
_B19 .set  204      +4
_B20 .set  208      +4
_B21 .set  212      +4
_B22 .set  216      +4
_B23 .set  220      +4
_B24 .set  224      +4
_B25 .set  228      +4
_B26 .set  232      +4
_B27 .set  236      +4
_B28 .set  240      +4
_B29 .set  244      +4
_B30 .set  248      +4
_B31 .set  252      +4
_AMR .set  256      +4
_CSR .set  260      +4
_IER .set  264      +4
_IRP .set  268      +4
_Framesize .set		272


BranchDelaySlot  	.set 	5

    .global _OSStartHighRdy
    .global _OSTCBHighRdy
    .global _OSTCBCur
    .global _OSPrioCur
    .global _OSPrioHighRdy
    .global _OSCtxSw
    .global _OSIntCtxSw
    .global _OSTaskSwHook
    .global _ReturnCurrentDP
    .global _getStackPtr
    .global _OSRunning

	.align	32
;***************************************************************************
; _ReturnCurrentDP
;
; Purpose:
;	Get the current data pointer.
;
; Notes:
;***************************************************************************
_ReturnCurrentDP:
        mv      DP,a4
        B       .S2     B3
        NOP     5

	.align	32
;***************************************************************************
; _getStackPtr
;
; Purpose:
;	Get the current stack pointer.
;
; Notes:  This is Jim Elliott's first TI 6414 assembly language function.
;***************************************************************************
_getStackPtr:
        mv      SP,a4
        B       .S2     B3
        NOP     5

	.align	32
;***************************************************************************
; _OSCtxSw
; _OSIntCtxSw
;
; Purpose:
;	Context switch.  Both OS and interrupt context switches are done here.
;
; Notes:
;	For questions, see app note about implementation.
;***************************************************************************
_OSIntCtxSw
_OSCtxSw
; Save the current context on it's stack
        addk    .s2 	(-_Framesize),SP
        STW     .D2T2   B0,*+SP(_B0)
       	STW     .D2T2   B1,*+SP(_B1)
       	STW     .D2T2   B2,*+SP(_B2)
       	STW     .D2T2   B3,*+SP(_B3)		; context return address
       	STW     .D2T2   B4,*+SP(_B4)
       	STW     .D2T2   B5,*+SP(_B5)
       	STW     .D2T2   B6,*+SP(_B6)
       	STW     .D2T2   B7,*+SP(_B7)
       	STW     .D2T2   B8,*+SP(_B8)
       	STW     .D2T2   B9,*+SP(_B9)
       	STW     .D2T2   B10,*+SP(_B10)
       	STW     .D2T2   B11,*+SP(_B11)
       	STW     .D2T2   B12,*+SP(_B12)
       	STW     .D2T2   B13,*+SP(_B13)
       	STW     .D2T2   B14,*+SP(_B14)	; DP
       	STW     .D2T2   B16,*+SP(_B16)
       	STW     .D2T2   B17,*+SP(_B17)
       	STW     .D2T2   B18,*+SP(_B18)
       	STW     .D2T2   B19,*+SP(_B19)
       	STW     .D2T2   B20,*+SP(_B20)
       	STW     .D2T2   B21,*+SP(_B21)
       	STW     .D2T2   B22,*+SP(_B22)
       	STW     .D2T2   B23,*+SP(_B23)
       	STW     .D2T2   B24,*+SP(_B24)
       	STW     .D2T2   B25,*+SP(_B25)
       	STW     .D2T2   B26,*+SP(_B26)
       	STW     .D2T2   B27,*+SP(_B27)
       	STW     .D2T2   B28,*+SP(_B28)
       	STW     .D2T2   B29,*+SP(_B29)
       	STW     .D2T2   B30,*+SP(_B30)
       	STW     .D2T2   B31,*+SP(_B31)

       	STW     .D2T1   A0,*+SP(_A0)
||      MV      SP,B0       ; Move current context stack pointer to B0
       	STW     .D2T1   A1,*+SP(_A1)
||      addk    (_Framesize),B0 ; Stack pointer AFTER popping off context
       	STW     .D2T1   A2,*+SP(_A2)
      	STW     .D2T2   B0,*+SP(_B15)	; We can directly restore this stack pointer after as switch
       	STW     .D2T1   A3,*+SP(_A3)
       	STW     .D2T1   A4,*+SP(_A4)
       	STW     .D2T1   A5,*+SP(_A5)
       	STW     .D2T1   A6,*+SP(_A6)
       	STW     .D2T1   A7,*+SP(_A7)
       	STW     .D2T1   A8,*+SP(_A8)
		STW     .D2T1   A9,*+SP(_A9)
       	STW     .D2T1   A10,*+SP(_A10)
       	STW     .D2T1   A11,*+SP(_A11)
       	STW     .D2T1   A12,*+SP(_A12)
       	STW     .D2T1   A13,*+SP(_A13)
       	STW     .D2T1   A14,*+SP(_A14)
		STW     .D2T1   A15,*+SP(_A15)
       	STW     .D2T1   A16,*+SP(_A16)
       	STW     .D2T1   A17,*+SP(_A17)
       	STW     .D2T1   A18,*+SP(_A18)
       	STW     .D2T1   A19,*+SP(_A19)
       	STW     .D2T1   A20,*+SP(_A20)
       	STW     .D2T1   A21,*+SP(_A21)
       	STW     .D2T1   A22,*+SP(_A22)
       	STW     .D2T1   A23,*+SP(_A23)
       	STW     .D2T1   A24,*+SP(_A24)
       	STW     .D2T1   A25,*+SP(_A25)
       	STW     .D2T1   A26,*+SP(_A26)
       	STW     .D2T1   A27,*+SP(_A27)
       	STW     .D2T1   A28,*+SP(_A28)
       	STW     .D2T1   A29,*+SP(_A29)
       	STW     .D2T1   A30,*+SP(_A30)
       	STW     .D2T1   A31,*+SP(_A31)
||      MVC     AMR,B0              ;  Save AMR, CSR, IER, IRP regs
        STW     .D2T2   B0,*+SP(_AMR)
	; Save CSR later, after PGIE set
||      MVC     IER,B2
        STW .D2T2   B2,*+SP(_IER)
	; IRP need not be saved

; Make sure PGIE is zero, interrupts are restored in critical exit.
; clear PGIE
        MVC     .S2     CSR,B4            ; |81|
        AND     .S2     -3,B4,B4          ; |81|
        MVC     .S2     B4,CSR            ; |81|
        NOP             1

; Save CSR
        MVC     CSR,B0
        NOP     6
        STW     .D2T2   B0,*+SP(_CSR)
        nop     6

; Set the stack pointer in the TCB
        MVKL    .S2     _OSTCBCur,B1
        MVKH    .S2     _OSTCBCur,B1
    	LDW     .D2T2   *+B1[0],B0     ; B0 = OSTCBCur
    	NOP		6
        STW     SP,*B0                 ; previous OSTCBCur->OSTCBStkPtr = SP ;

;Pop NEW current task's context

;***************************************************************************
; _OSStartHighRdy
;
; Purpose:
; 	Starts the next available task
;
; Notes:
;	This is an entry point for both the OS anc continuation of the
;	above context switch code.
;***************************************************************************

_OSStartHighRdy
;---------------------------------------;
;  Call User definable OSTaskSwHook();  ;
;---------------------------------------;
        B       .S1     _OSTaskSwHook
        NOP             3
; Set up the return address for task switch hook
        MVKL    .S2     OSStartHighRdy_1,B3		; move low 16-bits of address into B3
        MVKH    .S2     OSStartHighRdy_1,B3		; move high 16-bits of addr into B3

OSStartHighRdy_1

;-------------------------------------------;
;  Get the address of the TCB that is going ;
;  to execute.
; The next section performs 2 assignments
; using a long memory model.
; OSTCBCur  = OSTCBHighRdy
; OSPrioCur = OSPrioHighRdy
;-------------------------------------------;
        MVKL    .S2     _OSTCBHighRdy,B1  ; Get the source address, low order...
        MVKH    .S2     _OSTCBHighRdy,B1  ; and high order.
    	LDW     .D2T2   *+B1[0],B4        ; B4 = OSTCBHighRdy
        ; At this point the data is not yet in B4.  It will appear soon.

        MVKL    .S1     _OSPrioHighRdy,A1
        MVKH    .S1     _OSPrioHighRdy,A1
    	LDB     .D1T1   *+A1[0],A4 		  ; A4 = OSPrioHighRdy

        MVKL    .S2     _OSTCBCur,B1
        MVKH    .S2     _OSTCBCur,B1
        STW     .D2T2   B4,*+B1[0]  	  ; When store is done, OSTCBCur  = OSTCBHighRdy.
        ; Leave B4 intact, used later.

        MVKL    .S1     _OSPrioCur,A1
        MVKH    .S1     _OSPrioCur,A1
        STB     .D1T1   A4,*+A1[0]  	  ; When store is done, OSPrioCur = OSPrioHighRdy

        ; OSRunning = 1
||  	mvk      1,b1				 	  ; move constant into B1
        MVKL    .S2     _OSRunning,B0
        MVKH    .S2     _OSRunning,B0
        STB     .D2T2   B1,*+B0[0]  	  ; When store is done, OSRunning = 1

; Get the stack pointer for the task to execute.  It is in the first address of the TCB.
; StackPointer = OSTCBHighRdy->OSTCBStkPtr
    	LDW     .D2T2   *B4,SP			  ; restore task stack pointer here
        nop     8
    	LDW     .D2T2   *+SP(_B3),B3	  ; restore task return address here
        nop     6

; Will continue context restoration, B3 has return address.
    	LDW     .D2T2   *+SP(_AMR),B0
        LDW     .D2T2   *+SP(_CSR),B1
    	LDW     .D2T2   *+SP(_IER),B2
	; IRP and IER need not be restored
    	LDW     .D2T1   *+SP(_A0),A0
    	LDW     .D2T1   *+SP(_A1),A1
    	LDW     .D2T1   *+SP(_A2),A2
    	LDW     .D2T1   *+SP(_A3),A3
    	LDW     .D2T1   *+SP(_A4),A4
    	LDW     .D2T1   *+SP(_A5),A5
    	LDW     .D2T1   *+SP(_A6),A6
    	LDW     .D2T1   *+SP(_A7),A7
    	LDW     .D2T1   *+SP(_A8),A8
    	LDW     .D2T1   *+SP(_A9),A9
    	LDW     .D2T1   *+SP(_A10),A10
    	LDW     .D2T1   *+SP(_A11),A11
||      mvc     B0,AMR              	  ; Restore AMR
    	LDW     .D2T1   *+SP(_A12),A12
||      mvc     B1,CSR              	  ; Restore CSR
    	LDW     .D2T1   *+SP(_A13),A13
    	LDW     .D2T1   *+SP(_A14),A14
    	LDW     .D2T1   *+SP(_A15),A15
    	LDW     .D2T1   *+SP(_A16),A16
    	LDW     .D2T1   *+SP(_A17),A17
    	LDW     .D2T1   *+SP(_A18),A18
    	LDW     .D2T1   *+SP(_A19),A19
    	LDW     .D2T1   *+SP(_A20),A20
    	LDW     .D2T1   *+SP(_A21),A21
    	LDW     .D2T1   *+SP(_A22),A22
    	LDW     .D2T1   *+SP(_A23),A23
    	LDW     .D2T1   *+SP(_A24),A24
    	LDW     .D2T1   *+SP(_A25),A25
    	LDW     .D2T1   *+SP(_A26),A26
    	LDW     .D2T1   *+SP(_A27),A27
    	LDW     .D2T1   *+SP(_A28),A28
    	LDW     .D2T1   *+SP(_A29),A29
    	LDW     .D2T1   *+SP(_A30),A30
    	LDW     .D2T1   *+SP(_A31),A31
        nop     8

	; B0 restored later
	; B1 restored later
    	LDW     .D2T2   *+SP(_B2),B2
	; Do not restore B3, already contains return addr.
    	LDW     .D2T2   *+SP(_B4),B4
    	LDW     .D2T2   *+SP(_B5),B5
    	LDW     .D2T2   *+SP(_B6),B6
    	LDW     .D2T2   *+SP(_B7),B7
    	LDW     .D2T2   *+SP(_B8),B8
    	LDW     .D2T2   *+SP(_B9),B9
    	LDW     .D2T2   *+SP(_B10),B10
    	LDW     .D2T2   *+SP(_B11),B11
    	LDW     .D2T2   *+SP(_B12),B12
		LDW     .D2T2   *+SP(_B13),B13
		LDW     .D2T2   *+SP(_B14),B14	  ; DP
	; Do not restore stack pointer here
    	LDW     .D2T2   *+SP(_B16),B16
    	LDW     .D2T2   *+SP(_B17),B17
    	LDW     .D2T2   *+SP(_B18),B18
    	LDW     .D2T2   *+SP(_B19),B19
    	LDW     .D2T2   *+SP(_B20),B20
    	LDW     .D2T2   *+SP(_B21),B21
    	LDW     .D2T2   *+SP(_B22),B22
    	LDW     .D2T2   *+SP(_B23),B23
    	LDW     .D2T2   *+SP(_B24),B24
    	LDW     .D2T2   *+SP(_B25),B25
    	LDW     .D2T2   *+SP(_B26),B26
    	LDW     .D2T2   *+SP(_B27),B27
    	LDW     .D2T2   *+SP(_B28),B28
    	LDW     .D2T2   *+SP(_B29),B29
    	LDW     .D2T2   *+SP(_B30),B30
    	LDW     .D2T2   *+SP(_B31),B31

        MVC     B3,IRP
; Restore last scratch regs
    	LDW     .D2T2   *+SP(_B0),B0
    	LDW     .D2T2   *+SP(_B1),B1

; Pop one frame, SP should point to next free location
        addk    .s2 _Framesize,SP
        B       .S2 IRP
        NOP     BranchDelaySlot

	.end

